#ifndef ROS_MEC_ODOM_H
#define ROS_MEC_ODOM_H

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include <std_msgs/UInt8.h> 
#include <signal.h>
#include <math.h>
#include <sensor_msgs/Imu.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>
#include <ros/duration.h>
#include <string.h>
#include <string>
#include "ros_mec_msg/york.h"

using namespace std;


const double odom_pose_covariance[36] = {1e-3, 0, 0, 0, 0, 0, 
										0, 1e-3, 0, 0, 0, 0,
										0, 0, 1e6, 0, 0, 0,
										0, 0, 0, 1e6, 0, 0,
										0, 0, 0, 0, 1e6, 0,
										0, 0, 0, 0, 0, 1e3};
const double odom_pose_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, 
										0, 1e-3, 1e-9, 0, 0, 0,
										0, 0, 1e6, 0, 0, 0,
										0, 0, 0, 1e6, 0, 0,
										0, 0, 0, 0, 1e6, 0,
										0, 0, 0, 0, 0, 1e-9};
 
const double odom_twist_covariance[36] = {1e-3, 0, 0, 0, 0, 0, 
										0, 1e-3, 0, 0, 0, 0,
										0, 0, 1e6, 0, 0, 0,
										0, 0, 0, 1e6, 0, 0,
										0, 0, 0, 0, 1e6, 0,
										0, 0, 0, 0, 0, 1e3};
const double odom_twist_covariance2[36] = {1e-9, 0, 0, 0, 0, 0, 
										0, 1e-3, 1e-9, 0, 0, 0,
										0, 0, 1e6, 0, 0, 0,
										0, 0, 0, 1e6, 0, 0,
										0, 0, 0, 0, 1e6, 0,
										0, 0, 0, 0, 0, 1e-9};

class ros_mec_odom
{
	public:
		ros_mec_odom();

		void imu_callback(const sensor_msgs::Imu &imu);
		void mec_callback(const ros_mec_msg::york &msg);
		void odom_publisher();

	private:
		string robot_frame_id, gyro_frame_id;
		double filter_Vx_match, filter_Vy_match;
		bool publish_imu;
 
		ros::NodeHandle n;
		ros::Time current_time, last_time;
		int frame_num;

		sensor_msgs::Imu imu_msg;

		ros::Subscriber imu_sub, mec_sub;
		ros::Publisher imu_pub, odom_pub;
		tf::TransformBroadcaster odom_broadcaster;
        
		double x, y, th, vx, vy, vth, dt;
};

#endif
